package math;

public class Quaternion {


	private double w;
	private double x;
	private double y;
	private double z;
	
	
	/**
	 * 
	 * @param a coord w
	 * @param b coord x
	 * @param c coord c
	 * @param d coord d
	 */
	public Quaternion(double a,double b,double c,double d){
		this.w = a;
		this.x = b;
		this.y = c;
		this.z = d;
	}
	
	/**
	 * 
	 * @param q
	 */
	public Quaternion(Quaternion q){
		this.w = q.w;
		this.x = q.x;
		this.y = q.y;
		this.z = q.z;
		//this.normalise();
	}
	
	/**
	 * 
	 * @param axis axe of rotation
	 * @param angl of rotation
	 * 
	 */
	public Quaternion(Vector3D axis,double angle){
		double sin = Math.sin(angle/2);
		double cos = Math.cos(angle/2);
		
		x = axis.getX()*sin;
		y = axis.getY()*sin;
		z = axis.getZ()*sin;
		w = cos;
		this.normalise();
	}
	
	public Quaternion(double a,double b,double c ){
		Vector3D vx = new Vector3D(1,0,0); 
		Vector3D vy = new Vector3D(0,1,0);
		Vector3D vz = new Vector3D(0,0,1);
		
		Quaternion qx = new Quaternion(vx,a);
		Quaternion qy = new Quaternion(vy,b);
		Quaternion qz = new Quaternion(vz,c);
		
		qx.thisTime(qy);
		qz.thisTime(qx);
		this.w = qz.w;
		this.x = qz.x;
		this.y = qz.y;
		this.z = qz.z;
	}
	
	
	public void conjugate(){
		x= - x;
		y= - y;
		z= - z;
	}
	
	public double getModulus(){
		return (w*w + x*x + y*y + z*z );
	}
	
	public Quaternion time(Quaternion q){
		return new Quaternion( 
				w*q.w - x*q.x - y*q.y - z*q.z ,
				w*q.x + x*q.w + y*q.z - z*q.y ,
				w*q.y - x*q.z + y*q.w + z*q.x ,
				w*q.z + x*q.y - y*q.x + z*q.w );
	}
	
	public void thisTime(Quaternion q){
				double tmpa = w*q.w - x*q.x - y*q.y - z*q.z;
				double tmpb = w*q.x + x*q.w + y*q.z - z*q.y;
				double tmpc = w*q.y - x*q.z + y*q.w + z*q.x;
				double tmpd = w*q.z + x*q.y - y*q.x + z*q.w;
				
				w=tmpa;
				x=tmpb;
				y=tmpc;
				z=tmpd;
	}
	
	public Quaternion add(Quaternion q){
		return new Quaternion( w+q.w, x+q.x , y+q.y , z+q.y   );
	}
	
	
	public void thisTime(double a){
		w = w*a;
		x = x*a;
		y = y*a;
		z = z*a; 
		
	}
	public Quaternion time(double f){
		return new Quaternion(w*f,x*f,y*f,z*f);	
	}
	
	public void normalise(){
		this.thisTime(1/this.getModulus());
	}
	
	public Quaternion getInverse()
	{
		double mod = this.getModulus();
		Quaternion q = new Quaternion(this);
		q.conjugate();
		q.thisTime(mod*mod);
		return q;
	}
	
	public static void rotate(Quaternion rot, Quaternion q){
		rot = q.time( rot.time(q.getInverse()));
		rot.normalise();

	}
	
	
	
	public Matrice3D quatToRot3D(){
		double xx = x*x,	 yy = y*y,	zz=z*z; 
		double xy = x*y,     xz = x*z,  yz=y*z;
		double xw = w*x,	 yw = y*w,	zw=z*w;
		
		return new Matrice3D(
				1-yy-zz,	2*(xy-zw),	2*(xz+yw),
				2*(xy+zw),	1-xx-zz,	2*(yz-xw),
				2*(xz-yw),	2*(yz+xw),	1-xx-yy
		);
		
	}

	public Matrice4D quatToRot4D(){
		double xx = x*x,	 yy = y*y,	zz=z*z; 
		double xy = x*y,     xz = x*z,  yz=y*z;
		double xw = w*x,	 yw = y*w,	zw=z*w;
		
		return new Matrice4D(
				1-yy-zz  ,	2*(xy-zw),	2*(xz+yw),	0,
				2*(xy+zw),	1-xx-zz  ,	2*(yz-xw),	0,
				2*(xz-yw),	2*(yz+xw),	1-xx-yy  ,	0,
				0		 ,	0		 ,	0		 ,  1
		);
		
	}
	/**
	 * 
	 * @return double[0] = angle double[suite] = non normalised axis
	 */
	public double[] quatToAxisAngle()
	{
		
		return new double[]{Math.acos(w)*2, x,y,z };
	}

	@Override
	public String toString() {
		return "Quaternion [w=" + w + ", x=" + x + ", y=" + y + ", z=" + z
				+ "]";
	}

	
}
